int PIN_LBUMP = 2;
// //
int PIN_RBUMP = 4;

int PIN_LDIST = 2;
int PIN_MDIST = 3;
int PIN_RDIST = 4;

int PIN_LLED = 5;
int PIN_MLED = 6;
int PIN_RLED = 7;

int DIST_TRESHOLD = 140;

int SCAN_DELAY = 20;


boolean bumpLeft = false;
boolean bumpMiddle = false;
boolean bumpRight = false;

void setup(){

  /* Configure and init pins */
  pinMode(PIN_RBUMP, OUTPUT);
  pinMode(PIN_LBUMP, OUTPUT);

  pinMode(PIN_RLED, OUTPUT);
    pinMode(PIN_MLED, OUTPUT);
  pinMode(PIN_LLED, OUTPUT);

  /* Set pins to default states */
  digitalWrite(PIN_RLED, LOW);

  digitalWrite(PIN_LLED, LOW);
  digitalWrite(PIN_RBUMP, HIGH);
  digitalWrite(PIN_LBUMP, HIGH);
}

void loop(){

  bumpLeft = false;
    bumpMiddle = false;
  bumpRight = false;

  if( check_n_bump(PIN_LDIST) ) bumpLeft = true; 
     if( check_n_bump(PIN_MDIST) ) bumpMiddle = true; 
  if( check_n_bump(PIN_RDIST) ) bumpRight = true; 


 /* Simulate bumpers */
  if(bumpLeft  || bumpMiddle){
    digitalWrite(PIN_LBUMP, LOW);
  } 
  else {
    digitalWrite(PIN_LBUMP, HIGH);
  }
  
  
 if(bumpRight  || bumpMiddle){
    digitalWrite(PIN_RBUMP, LOW);
  } 
  else {
    digitalWrite(PIN_RBUMP, HIGH);
  }



 if(bumpRight){
    digitalWrite(PIN_RLED, HIGH);
  } 
  else {
    digitalWrite(PIN_RLED, LOW);
  }

 if(bumpLeft){
    digitalWrite(PIN_LLED, HIGH);
  } 
  else {
    digitalWrite(PIN_LLED, LOW);
  }
  
   if(bumpMiddle){
    digitalWrite(PIN_MLED, HIGH);
  } 
  else {
    digitalWrite(PIN_MLED, LOW);
  }



  delay(SCAN_DELAY); // Wait a while

}

boolean check_n_bump(int theSensorPin){
  int val = analogRead(theSensorPin);
  if(val > DIST_TRESHOLD) return true;
  return false;
}



